#ifndef __ROB_MAIN_H__
#define __ROB_MAIN_H__

/* System Includes */
#include <stm32f10x.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

/* EASML Library Includes */
#include "common.h"
#include "mfl.h"
#include "efx.h"
#include "ifx.h"
#include "ifl.h"
#include "mfx.h"
#include "bfx.h"
#include "crc.h"

/* Robot Arm Module Includes */
#include "rob_common.h"
#include "rob_kinematics.h"
#include "rob_servo.h"
#include "rob_sensor.h"
#include "rob_communication.h"
#include "rob_control.h"

/* Hardware Configuration */
#define ROB_SYSTEM_CLOCK_HZ 72000000u
#define ROB_TIMER_PRESCALER 72u
#define ROB_PWM_FREQUENCY_HZ 50u

/* Version Information */
#define ROB_VERSION_MAJOR 1u
#define ROB_VERSION_MINOR 0u
#define ROB_VERSION_PATCH 0u
#define ROB_VERSION_BUILD 1u
#define ROB_VERSION_STRING "v1.0.0-beta"

/* Hardware Pin Definitions (STM32F103C8T6) */

/* Servo Control Pins */
#define ROB_SERVO0_PIN      GPIO_Pin_3
#define ROB_SERVO0_PORT     GPIOB
#define ROB_SERVO0_CLK      RCC_APB2Periph_GPIOB

#define ROB_SERVO1_PIN      GPIO_Pin_8
#define ROB_SERVO1_PORT     GPIOB
#define ROB_SERVO1_CLK      RCC_APB2Periph_GPIOB

#define ROB_SERVO2_PIN      GPIO_Pin_9
#define ROB_SERVO2_PORT     GPIOB
#define ROB_SERVO2_CLK      RCC_APB2Periph_GPIOB

#define ROB_SERVO3_PIN      GPIO_Pin_6
#define ROB_SERVO3_PORT     GPIOB
#define ROB_SERVO3_CLK      RCC_APB2Periph_GPIOB

#define ROB_SERVO4_PIN      GPIO_Pin_7
#define ROB_SERVO4_PORT     GPIOB
#define ROB_SERVO4_CLK      RCC_APB2Periph_GPIOB

#define ROB_SERVO5_PIN      GPIO_Pin_4
#define ROB_SERVO5_PORT     GPIOB
#define ROB_SERVO5_CLK      RCC_APB2Periph_GPIOB

/* Sensor Pins */
#define ROB_TOUCH_PIN       GPIO_Pin_1
#define ROB_TOUCH_PORT      GPIOA
#define ROB_TOUCH_CLK       RCC_APB2Periph_GPIOA

#define ROB_IR_PIN          GPIO_Pin_0
#define ROB_IR_PORT         GPIOA
#define ROB_IR_CLK          RCC_APB2Periph_GPIOA

#define ROB_ULTRASONIC_TRIG_PIN     GPIO_Pin_0
#define ROB_ULTRASONIC_TRIG_PORT    GPIOB
#define ROB_ULTRASONIC_TRIG_CLK     RCC_APB2Periph_GPIOB

#define ROB_ULTRASONIC_ECHO_PIN     GPIO_Pin_2
#define ROB_ULTRASONIC_ECHO_PORT    GPIOA
#define ROB_ULTRASONIC_ECHO_CLK     RCC_APB2Periph_GPIOA

#define ROB_SOUND_PIN       GPIO_Pin_1
#define ROB_SOUND_PORT      GPIOB
#define ROB_SOUND_CLK       RCC_APB2Periph_GPIOB

/* LED and Buzzer Pins */
#define ROB_LED_PIN         GPIO_Pin_13
#define ROB_LED_PORT        GPIOB
#define ROB_LED_CLK         RCC_APB2Periph_GPIOB

#define ROB_BUZZER_PIN      GPIO_Pin_5
#define ROB_BUZZER_PORT     GPIOB
#define ROB_BUZZER_CLK      RCC_APB2Periph_GPIOB

#define ROB_RGB_PIN         GPIO_Pin_6
#define ROB_RGB_PORT        GPIOA
#define ROB_RGB_CLK         RCC_APB2Periph_GPIOA

/* Communication Pins */
#define ROB_UART1_TX_PIN    GPIO_Pin_9
#define ROB_UART1_TX_PORT   GPIOA
#define ROB_UART1_RX_PIN    GPIO_Pin_10
#define ROB_UART1_RX_PORT   GPIOA
#define ROB_UART1_CLK       RCC_APB2Periph_GPIOA

#define ROB_UART3_TX_PIN    GPIO_Pin_10
#define ROB_UART3_TX_PORT   GPIOB
#define ROB_UART3_RX_PIN    GPIO_Pin_11
#define ROB_UART3_RX_PORT   GPIOB
#define ROB_UART3_CLK       RCC_APB2Periph_GPIOB

/* I2C Pins (Software I2C) */
#define ROB_I2C_SCL_PIN     GPIO_Pin_5
#define ROB_I2C_SCL_PORT    GPIOA
#define ROB_I2C_SDA_PIN     GPIO_Pin_7
#define ROB_I2C_SDA_PORT    GPIOA
#define ROB_I2C_CLK         RCC_APB2Periph_GPIOA

/* PS2 Controller Pins */
#define ROB_PS2_DAT_PIN     GPIO_Pin_15
#define ROB_PS2_DAT_PORT    GPIOA
#define ROB_PS2_CMD_PIN     GPIO_Pin_14
#define ROB_PS2_CMD_PORT    GPIOA
#define ROB_PS2_ATT_PIN     GPIO_Pin_13
#define ROB_PS2_ATT_PORT    GPIOA
#define ROB_PS2_CLK_PIN     GPIO_Pin_12
#define ROB_PS2_CLK_PORT    GPIOA
#define ROB_PS2_GPIO_CLK    RCC_APB2Periph_GPIOA

/* Button Pins */
#define ROB_KEY1_PIN        GPIO_Pin_8
#define ROB_KEY1_PORT       GPIOA
#define ROB_KEY2_PIN        GPIO_Pin_11
#define ROB_KEY2_PORT       GPIOA
#define ROB_KEY_CLK         RCC_APB2Periph_GPIOA

/* Timer Definitions */
#define ROB_TIMER_PWM       TIM2
#define ROB_TIMER_PWM_CLK   RCC_APB1Periph_TIM2
#define ROB_TIMER_SYSTICK   TIM3
#define ROB_TIMER_SYSTICK_CLK RCC_APB1Periph_TIM3

/* ADC Definitions */
#define ROB_ADC1_CLK        RCC_APB2Periph_ADC1
#define ROB_ADC_CHANNELS    4u

/* USART Definitions */
#define ROB_USART1_CLK      RCC_APB2Periph_USART1
#define ROB_USART1_BAUD     115200u
#define ROB_USART3_CLK      RCC_APB1Periph_USART3
#define ROB_USART3_BAUD     9600u

/* SPI Flash Definitions */
#define ROB_SPI_FLASH_CS_PIN    GPIO_Pin_4
#define ROB_SPI_FLASH_CS_PORT   GPIOA
#define ROB_SPI_FLASH_CLK       RCC_APB2Periph_GPIOA

/* Default Robot Configuration */
#define ROB_DEFAULT_L0      100.0f  /* Base height (mm) */
#define ROB_DEFAULT_L1      105.0f  /* First link length (mm) */
#define ROB_DEFAULT_L2      88.0f   /* Second link length (mm) */
#define ROB_DEFAULT_L3      155.0f  /* End effector length (mm) */

/* Default Servo Angles (degrees) */
#define ROB_DEFAULT_SERVO_ANGLES { 0.0f, -90.0f, 0.0f, 0.0f, 0.0f, 0.0f }

/* Default Joint Limits (degrees) */
#define ROB_DEFAULT_JOINT_LIMITS { \
    {-135.0f, 135.0f}, /* Base rotation */ \
    {-90.0f, 90.0f},   /* Shoulder */ \
    {-135.0f, 135.0f}, /* Elbow */ \
    {-90.0f, 90.0f},   /* Wrist pitch */ \
    {-180.0f, 180.0f}, /* Wrist roll */ \
    {0.0f, 180.0f}     /* Gripper */ \
}

/* System Configuration Structure */
typedef struct {
    uint32 SystemClockHz_u32;
    uint16 ControlCycleMs_u16;
    uint16 CommTimeoutMs_u16;
    boolean EnableSafety_b;
    boolean EnableLogging_b;
    uint8 Reserved_au8[16];
} Rob_SystemConfig_Type;

/* Global System Instance */
extern Rob_ControlSystem_Type g_RobotSystem;
extern Rob_SystemConfig_Type g_SystemConfig;

/* Function Declarations */

/* System Initialization */
Rob_Status_Type Rob_SystemInit(void);
Rob_Status_Type Rob_HardwareInit(void);
Rob_Status_Type Rob_PeripheralsInit(void);

/* Clock and Timer Functions */
void Rob_ClockInit(void);
void Rob_SysTickInit(void);
void Rob_TimerInit(void);
uint32 Rob_GetSystemTime_u32(void);
void Rob_DelayMs(uint32 DelayMs_u32);

/* GPIO Initialization */
void Rob_GpioInit(void);
void Rob_ServoGpioInit(void);
void Rob_SensorGpioInit(void);
void Rob_LedGpioInit(void);
void Rob_ButtonGpioInit(void);

/* Communication Initialization */
void Rob_UartInit(void);
void Rob_I2cInit(void);
void Rob_AdcInit(void);

/* Interrupt Functions */
void Rob_InterruptInit(void);
void Rob_InterruptEnable(void);
void Rob_InterruptDisable(void);

/* Main Application Loop */
void Rob_MainLoop(void);
Rob_Status_Type Rob_ProcessTasks(void);

/* Debug and Logging Functions */
void Rob_Printf(const char *format, ...);
void Rob_LogError(const char *message);
void Rob_LogInfo(const char *message);
void Rob_LogDebug(const char *message);

/* System Control Functions */
void Rob_SystemReset(void);
void Rob_SystemSleep(void);
void Rob_SystemWakeup(void);

/* Configuration Functions */
Rob_Status_Type Rob_LoadDefaultConfig(void);
Rob_Status_Type Rob_SaveConfig(void);
Rob_Status_Type Rob_LoadConfig(void);

/* Utility Macros */
#define ROB_SET_BIT(REG, BIT)     ((REG) |= (BIT))
#define ROB_CLEAR_BIT(REG, BIT)   ((REG) &= ~(BIT))
#define ROB_READ_BIT(REG, BIT)    ((REG) & (BIT))
#define ROB_CLEAR_REG(REG)        ((REG) = (0x0))
#define ROB_WRITE_REG(REG, VAL)   ((REG) = (VAL))
#define ROB_READ_REG(REG)         ((REG))

/* LED Control Macros */
#define ROB_LED_ON()    GPIO_ResetBits(ROB_LED_PORT, ROB_LED_PIN)
#define ROB_LED_OFF()   GPIO_SetBits(ROB_LED_PORT, ROB_LED_PIN)
#define ROB_LED_TOGGLE() GPIO_WriteBit(ROB_LED_PORT, ROB_LED_PIN, \
                        (BitAction)(1 - GPIO_ReadOutputDataBit(ROB_LED_PORT, ROB_LED_PIN)))

/* Buzzer Control Macros */
#define ROB_BUZZER_ON()  GPIO_SetBits(ROB_BUZZER_PORT, ROB_BUZZER_PIN)
#define ROB_BUZZER_OFF() GPIO_ResetBits(ROB_BUZZER_PORT, ROB_BUZZER_PIN)

/* Button Reading Macros */
#define ROB_KEY1_READ()  GPIO_ReadInputDataBit(ROB_KEY1_PORT, ROB_KEY1_PIN)
#define ROB_KEY2_READ()  GPIO_ReadInputDataBit(ROB_KEY2_PORT, ROB_KEY2_PIN)

/* Servo Pin Control Macros */
#define ROB_SERVO_PIN_SET(index, level) do { \
    switch(index) { \
        case 0: GPIO_WriteBit(ROB_SERVO0_PORT, ROB_SERVO0_PIN, level); break; \
        case 1: GPIO_WriteBit(ROB_SERVO1_PORT, ROB_SERVO1_PIN, level); break; \
        case 2: GPIO_WriteBit(ROB_SERVO2_PORT, ROB_SERVO2_PIN, level); break; \
        case 3: GPIO_WriteBit(ROB_SERVO3_PORT, ROB_SERVO3_PIN, level); break; \
        case 4: GPIO_WriteBit(ROB_SERVO4_PORT, ROB_SERVO4_PIN, level); break; \
        case 5: GPIO_WriteBit(ROB_SERVO5_PORT, ROB_SERVO5_PIN, level); break; \
    } \
} while(0)

/* Error Handling Macros */
#define ROB_ASSERT(expr) do { \
    if (!(expr)) { \
        Rob_LogError("Assertion failed: " #expr); \
        while(1); \
    } \
} while(0)

#define ROB_CHECK_STATUS(status) do { \
    if ((status) != ROB_STATUS_OK) { \
        Rob_LogError("Status error: " #status); \
        return (status); \
    } \
} while(0)

#endif /* __ROB_MAIN_H__ */
